#include "kinematics.h"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <unistd.h>


#define RATIO(x) x * 2.0

static double theta = 135.0;

static double link_ac = RATIO(27.0);
static double link_cd = RATIO(36.0);
static double link_ab = RATIO(36.0);
static double link_be = RATIO(46.0);
static double link_de = RATIO(46.0);
static double link_ef = RATIO(16.0);

static struct Point_t point_c ;
static struct Point_t point_a ;

#define deg2rad(deg) (M_PI * deg / 180.0)
#define rad2deg(rad) (rad * 180.0 / M_PI)
#define link_bf sqrt(link_be * link_be + link_ef * link_ef - 2 * link_be * link_ef * cos(M_PI * theta / 180.0))

static void init(){
    point_a.x = -link_ac / 2;
    point_a.y = RATIO(0);
    point_c.x = link_ac / 2;
    point_c.y = RATIO(0);
}

static double calc_radians_between_vector(struct Vec_t a, struct Vec_t b) {
    return atan2(a.x * b.y - a.y * b.x, a.x * b.x + a.y * b.y);
}


/**
 *
 * @param p0: 圆0的圆心
 * @param p1: 圆1的圆心
 * @param r0: 圆0的半径
 * @param r1: 圆1的半径
 * @return: 交点
 */
static struct Points_t* calculate_intersection(struct Point_t * p0, struct Point_t * p1, double r0, double r1) {
    struct Points_t* points = malloc(sizeof(struct Points_t));

    // 圆心距离
    double d = sqrt(pow(p0->x - p1->x, 2) + pow(p0->y - p1->y, 2));
    // 半径之和
    double r = r0 + r1;

    // 一个圆在另一个圆内，则没有交点
    if (d < fabs(r0 - r1)) {
        points->len = 0;
        return points;
    }

    // 如果两圆相离，则没有交点
    if (d > r) {
        points->len = 0;
        return points;
    }

    // 如果两圆相切，则有一个交点
    if (d == r) {
        points->len = 1;
        points->points = malloc(sizeof(struct Point_t));
        points->points->x = p0->x + (p1->x - p0->x) * (r0 / r);
        points->points->y =  p0->y + (p1->y - p0->y) * (r0 / r);
        return points;
    }
    // 如果两圆相交，则有两个交点
    double a = (r0 * r0 - r1 * r1 + d * d) / (2 * d);

    double h0 = sqrt(r0 * r0 - a * a);

    double p2x = p0->x + (p1->x - p0->x) * a / d;
    double p2y = p0->y + (p1->y - p0->y) * a / d;

    double x3 = p2x + h0 * (p1->y - p0->y) / d;
    double y3 = p2y - h0 * (p1->x - p0->x) / d;

    double x4 = p2x - h0 * (p1->y - p0->y) / d;
    double y4 = p2y + h0 * (p1->x - p0->x) / d;

    points->len = 2;
    points->points = malloc(2 * sizeof(struct Point_t));
    struct Point_t p3 = {x3, y3};
    struct Point_t p4 = {x4, y4};
    points->points[0] = p3;
    points->points[1] = p4;

//    memcpy(points->points, &p3, sizeof(Point_t));
//    memcpy(&(points->points)[1], &p4, sizeof(Point_t));

    return points;
}

double* inverse(struct Point_t temp_point_end)
{
    init();

    temp_point_end.x = RATIO(temp_point_end.x);
    temp_point_end.y = RATIO(temp_point_end.y);
    // 计算左交点(根据A-B圆和F-B圆的交点B)
    struct Points_t *intersection_left_points = calculate_intersection(&point_a, &temp_point_end, link_ab, link_bf);
    if(intersection_left_points->len == 0) {
        printf("------------求左交点B失败！\n");
        return NULL;
    }

    struct Point_t point_b;
    point_b = intersection_left_points->points[0];
    if (intersection_left_points->len == 2) {
        if (intersection_left_points->points[1].x < intersection_left_points->points[0].x) {
            point_b = intersection_left_points->points[1];
        }
    }
    free(intersection_left_points->points);
    free(intersection_left_points);

    // 计算点E位置(根据B-E圆和F-E圆的交点E)
    struct Points_t *e_points = calculate_intersection(&point_b, &temp_point_end, link_be, link_ef);
    if (e_points->len == 0) {
        printf("------------求交点E失败！\n");
        return NULL;
    }
    // 向量BF要在向量BE的顺时针位(BE方向的右侧）
    struct Vec_t vec_bf;
    vec_bf.x = temp_point_end.x - point_b.x;
    vec_bf.y = temp_point_end.y - point_b.y;

    double dx = e_points->points[0].x;
    double dy = e_points->points[0].y;
    struct Vec_t vec_bd;
    vec_bd.x = dx - point_b.x;
    vec_bd.y = dy - point_b.y;

    double vmin = calc_radians_between_vector(vec_bf, vec_bd);
    struct Point_t point_e;
    point_e = e_points->points[0];

    printf("point e: %f %f\n", point_e.x, point_e.y);

    for (int i = 1; i < e_points->len; ++i) {
        dx = e_points->points[0].x;
        dy = e_points->points[0].y;
        vec_bd.x = dx - point_b.x;
        vec_bd.y = dy - point_b.y;

        double v = calc_radians_between_vector(vec_bf, vec_bd);
        if (v < vmin) {
            vmin = v;

            point_e = e_points->points[i];
        }
    }
    free(e_points->points);
    free(e_points);

    // 计算右交点
    struct Points_t *intersection_right_points = calculate_intersection(&point_e, &point_c, link_de, link_cd);
    if(intersection_right_points->len == 0) {
        printf("------------求右交点D失败！\n");
        return NULL;
    }

    struct Point_t point_d;
    point_d = intersection_right_points->points[0];
    if (intersection_right_points->len == 2) {
        if (intersection_right_points->points[1].x > intersection_right_points->points[0].x) {
            point_d = intersection_right_points->points[1];
        }
    }
    free(intersection_right_points->points);
    free(intersection_right_points);

    struct Vec_t vec_ab = {point_b.x - point_a.x, point_b.y - point_a.y};
    struct Vec_t vec_x_negative = {-1, 0};

    struct Vec_t vec_cd = {point_d.x - point_c.x, point_d.y - point_c.y};
    struct Vec_t vec_x_positive = {1, 0};

    double alpha = calc_radians_between_vector(vec_ab, vec_x_negative);
    double beta = calc_radians_between_vector(vec_cd, vec_x_negative);

    double * ret = malloc(2 * sizeof(double));
    ret[0] = rad2deg(alpha);
    ret[1] = rad2deg(beta);
    return ret;
}


